#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
import ros_numpy
import numpy as np
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PoseStamped,Pose
from ros_numpy import numpify,msgify
import open3d as o3d


def pc2_msg_to_np(pc_msg):
    pc_np=numpify(pc_msg)
    pc_np= np.stack([pc_np[f] for f in ('x', 'y', 'z')])

    return pc_np

def np_to_pc2_msg(pc_np):
    data=np.zeros(pc_np.shape[1],
    dtype=[('x',np.float32), ('y',np.float32), ('z',np.float32)])
    data['x']=pc_np[0,]
    data['y']=pc_np[1,]
    data['z']=pc_np[2,]

    pc_msg=msgify(PointCloud2,data)
    pc_msg.is_dense=False
        
    return pc_msg




PC_SIZE=233

pc_random= np.random.uniform(-2, 2, size=(3, PC_SIZE))
def talker():
    
    pc_pub=rospy.Publisher('/slot/pointcloud',PointCloud2,queue_size=10)
    target_pose_pub=rospy.Publisher('/slot/target/pose',PoseStamped,queue_size=10)
    mav_pose_pub=rospy.Publisher('/slot/mav/pose',PoseStamped,queue_size=10)
    rospy.init_node('sim_data_generator', anonymous=True)
    rate = rospy.Rate(50) # 500hz
    
    theta=0
    r=0.5
    while not rospy.is_shutdown():
        theta=theta+0.06
        mx=np.cos(theta+1.3)*r*1
        my=np.sin(theta+1.3)*r*1
        mz=0
        tx=np.cos(theta*1.323)*r*1+5
        ty=np.sin(theta*1.323)*r*1
        tz=0
        
    
        
        pc_c=np.array([8,0,0]).reshape([3,1])
        #pc_random= np.random.uniform(-10, 10, size=(3, PC_SIZE))
        pc_np=pc_c+pc_random
        
        pc_msg=np_to_pc2_msg(pc_np)
        pc_msg.header.frame_id="world"
        pc_pub.publish(pc_msg)
        
        #target pose
        noise_scale=0.
        ns=noise_scale
        target_pose=PoseStamped()
        
        target_pose_np=np.array([[1,0,0,tx+np.random.uniform(-ns,ns)],
                                 [0,1,0,ty+np.random.uniform(-ns,ns)],
                                 [0,0,1,tz+np.random.uniform(-ns,ns)],
                                 [0,0,0,1]])
        target_pose.pose=msgify(Pose,target_pose_np)
        target_pose.header.frame_id="world"
        target_pose_pub.publish(target_pose)
        
        #target pose
        mav_pose=PoseStamped()
        mav_pose_np=np.array([[1,0,0,mx+np.random.uniform(-ns,ns)],
                              [0,1,0,my+np.random.uniform(-ns,ns)],
                              [0,0,1,mz+np.random.uniform(-ns,ns)],
                              [0,0,0,1]])
        mav_pose.pose=msgify(Pose,mav_pose_np)
        mav_pose.header.frame_id="world"
        mav_pose_pub.publish(mav_pose)

            
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
